#! /usr/bin/python

import cwiid
import time
import liblo
import euclid
import math

ROT_CAL = 5.81 / 6500.0

BUTTON_HOME  =  128
BUTTON_UP    = 2048
BUTTON_DOWN  = 1024
BUTTON_LEFT  =  256
BUTTON_RIGHT =  512

buttons = {'fire'      : (1,    4, 0),
           'open'      : (2,    8, 0),
           'left'      : (3,  256, 0),
           'right'     : (4,  512, 0),
           'weapon_up' : (8, 4096, 0),
           'weapon_dn' : (7,   16, 0)
        }

host = "10.0.1.28"
port = 7335
addr = liblo.Address(host, port, liblo.UDP)

print "Press 1+2 on the wiimote"
wm = cwiid.Wiimote()
wm.led = 1
wm.enable(cwiid.FLAG_MOTIONPLUS)
wm.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_MOTIONPLUS
print "OK, connected"
print "Keep wiimote still - calibrating zero values ..."
time.sleep(1)

# calibrate
calibration_offset = 0
x0 = y0 = z0 = 0
button_b_state = 0
button_a_state = 0
for x in xrange(0, 50):
    (xn, yn, zn) = wm.state['motionplus']['angle_rate']
    x0 += xn
    y0 += yn
    z0 += zn

    time.sleep(0.05)

x0 = x0 / 50
y0 = y0 / 50
z0 = z0 / 50

print "done."

q = euclid.Quaternion()

framestart = time.time()
forwardVector = euclid.Vector3(0.0, 0.0, 1.0)

while True:
    #print wm.state
    (x, y, z) = wm.state['motionplus']['angle_rate']
    currtime = time.time()
    timedelta = currtime - framestart
    framestart = currtime

    x = (x - x0) * ROT_CAL * timedelta
    y = (y - y0) * ROT_CAL * -1.0 * timedelta
    z = (z - z0) * ROT_CAL * timedelta

    # do button stuff
    buttonState = wm.state['buttons']
    for buttonName in buttons.keys():
        (buttonNum, wiimoteIndex, state) = buttons[buttonName]

        newState = buttonState & wiimoteIndex
        if state != newState:
            buttons[buttonName] = (buttonNum, wiimoteIndex, newState)
            if newState:
                liblo.send(addr, '/button-down', buttonNum)
            else:
                liblo.send(addr, '/button-up', buttonNum)

    if buttonState & BUTTON_HOME:
        q = euclid.Quaternion()

    if buttonState & BUTTON_DOWN:
        stickY = 1.0
    elif buttonState & BUTTON_UP:
        stickY = -1.0
    else:
        stickY = 0.0

    liblo.send(addr, '/value', 'joyY', stickY)
    q.rotate_euler(z, y, x)
    heading = q * forwardVector
    angle = -1.0 * math.atan2(heading[0], -1.0 * heading[2]) / math.pi
    print heading, angle

    liblo.send(addr, '/value', 'rotX', angle)

    time.sleep(0.025)



